#pragma once
#include "pid.h"
#include "adrc.h"
#include "can.h"
#include "encoder.h"
#include "adrc.h"
#include "kalman.h"
#include "can_interface.h"


class GM6020 {
public:
    float m_speed_target;
    float m_speed_current;
    float m_angle_current;
    float m_angle_target;
    float m_angle_current_encoder;
    float m_speed_current_encoder;
    float m_angle_current_encoder_filter;
    float m_speed_current_encoder_filter;
    float m_angle_current_external;
    float m_speed_current_external;

    // motor reducer reduction ratio
	float m_reduction_ratio; 

    // control algorithms
    Adrc_TD* m_angle_td;
    Pid* m_speed_pid;
    Pid* m_angle_pid;
    uint8_t step_ff_key;


    // filer algorithms
    Kalman* m_kalman_filter_angle;
    Kalman* m_kalman_filter_speed;
    Kalman* m_kalmab_filter_angle_des;

    CAN_TypeDef* m_CANx;
    AbsEncoder* m_encoder;
    uint32_t m_id;
    uint32_t m_state_update_times;


    void SpeedUpdate(uint8_t type, float input);
    void EncoderSpeedUpdate(float speed);
    void AngleUpdate(uint8_t type, float input);
    void EncoderAngleUpdate(uint32_t value);
    void SpeedControl(void);
    void AngleControl(void);

    typedef struct 
    {
        float start_angle;//补偿角度起始值
        float end_angle;//补偿角度结束值
        float speed_dead_zone;//速度死区
        uint8_t table_length;//补偿表长度
        float gra_table_data[20];//重力补偿数据
        float fri_table_data[20];//摩擦补偿数据，在前馈参数初始化中赋值，需要根据实际情况测量
    }FeedForwardTable;
    FeedForwardTable ff_table;
    int GetGravityFeedForward(float angle_current);
    int GetFrictionFeedForward(float angle_current, float speed_current);
    

    GM6020(CAN_TypeDef* CANx, uint32_t id, float ration) { 
        m_CANx = CANx;
        m_id = id;
        m_reduction_ratio = ration;
        m_state_update_times = 0;
        m_speed_target = 0;
        m_angle_target = 0;
        m_speed_current = 0;
        m_angle_current = 0;
        m_angle_current_encoder = 0;
        m_speed_current_encoder = 0;
        m_angle_current_encoder_filter = 0;
        m_speed_current_encoder_filter = 0;
        m_angle_current_external = 0;
        m_speed_current_external = 0;
    };
    ~GM6020();
};
